#include <dispatch_system/moying_map.h>
#include <dispatch_system/map_pretreatment.h>


namespace moying
{
namespace navigation
{

    log4cplus::Logger logger;
    log4cplus::Initializer initializer;

    MoyingMap::MoyingMap()
    {
        std::string log_cfg_path("./");
        log_cfg_path += "configs/traffic_manager/logger.properties";
        log4cplus::PropertyConfigurator::doConfigure(log_cfg_path); 
        logger = log4cplus::Logger::getInstance(LOG4CPLUS_TEXT("main"));

        size_th_ = 8;
        size_sm_ = 5;
    }

    MoyingMap::~MoyingMap()
    {
    }

    CostMapPtr MoyingMap::getCostMap()
    {
        return costmap_;
    }

    MapDimension MoyingMap::getDimensions(){
        MapDimension d;
        d.x     = size_x_;
        d.y     = size_y_;
        d.theta = size_th_;
        d.time  = time_counts_;
        d.sm_state  = size_sm_;
        return d;
    }

    bool MoyingMap::init(const CostMapPtr &costmap){

        sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();

        if(costmap == nullptr)
            return false;

        costmap_ = costmap;
        size_x_ = costmap->getSizeInCellsX();
        size_y_ = costmap->getSizeInCellsY();


        const auto& tr_cfg = cfg_info.traffic_manager_cfg;
        float path_plan_map_res = tr_cfg.path_plan_map_resolution;
        float robot_nav_speed = tr_cfg.robot_nav_speed;
        int time_map_length = tr_cfg.time_map_length;
        time_map_length = (time_map_length <= 0 || time_map_length >= 1000) ? 600 : time_map_length;
        robot_nav_speed = (robot_nav_speed <= 0 || robot_nav_speed >= 3) ? 0.5 : robot_nav_speed;
        path_plan_map_res = (path_plan_map_res <= 0 || path_plan_map_res > 1) ? 0.5 : path_plan_map_res;
        float time_res = 1/(robot_nav_speed/path_plan_map_res);

        path_plan_param_.nav_speed = robot_nav_speed;
        path_plan_param_.map_plan_resolution = path_plan_map_res;
        path_plan_param_.time_map_resolution = time_res;
        
        // if(!initializeTimedMap(600, 1))
        if(!initializeTimedMap(time_map_length, time_res))
        {
            ERROR("Initialize moying map failed");
            return false;
        }

        /****************预处理地图cell,更正离障碍物较近的cell的导航坐标*****************/
        float robot_len = 1;
        float robot_width = 0.618;
        for(auto rs : cfg_info.robot_size_cfgs){
            if(rs.robot_type.compare("1")){
                robot_len   = rs.length;
                robot_width = rs.width;
                break;
            }
        }

        WARNING("start pretreatment map "<<time(0));

        std::vector<std::pair<double,double>> robot_sizes;
        for(auto size_cfg : cfg_info.robot_size_cfgs){
            auto size = std::make_pair<double,double>(size_cfg.length,size_cfg.width);
            bool contained = false;
            for(auto item : robot_sizes){
                if(item.first == size.first && item.second == size.second){
                    contained = true;
                    break;
                }
            }
            if(contained)
                continue;
            robot_sizes.emplace_back(size);
        }

        LOG_INFO("robot_size count:"<<robot_sizes.size()<<" size:"<<cfg_info.robot_size_cfgs.size());
        int count = robot_sizes.size();
        corrected_cells_in_size_.resize(count);
        printf("robot size coutn:%d\n",count);
        PretreatmentMap pm(this);
        for(int i=0;i < count;i++){
            auto robot_size = robot_sizes[i];
            double length   = robot_size.first;
            double width    = robot_size.second;
            float l_p = tr_cfg.robot_length_padding + tr_cfg.cell_correct_padding;
            float w_p = tr_cfg.robot_width_padding + tr_cfg.cell_correct_padding;
            pm.setRobotSize(length,width,l_p,w_p);
            
            CorrectCellInSize& ccis = corrected_cells_in_size_[i];
            ccis.vehicle_length = length;
            ccis.vehicle_width  = width;

            CorrectCells& ccs = ccis.cells;

            if( pm.correctMapCellCoordinate(ccs,size_x_,size_y_) ){

            }

            /*****************/
            WARNING("stop pretreatment map "<<time(0));


            /*********添加运动模式********/
            ccis.custom_move_patterns   = std::make_shared<CustomMovePatterns>(this);
            auto custom_move_patterns = ccis.custom_move_patterns;

            MovePatternsPtr custom_moves = std::make_shared<MovePatterns>();
            custom_moves->reserve(20);
            custom_moves->push_back(kMoveForwardPattern);
            custom_moves->push_back(kReduceSpeedPattern);
            custom_moves->push_back(kTurnLeftPattern);
            custom_moves->push_back(kTurnRightPattern);
            custom_moves->push_back(kLeftFrontPattern);
            custom_moves->push_back(kRightFrontPattern);

            MovePatternsPtr custom_backward_moves = std::make_shared<MovePatterns>();
            custom_backward_moves->reserve(20);
            custom_backward_moves->push_back(kMoveBackwardPattern);
            custom_backward_moves->push_back(kAccelerateBackPattern);
            custom_backward_moves->push_back(kReduceSpeedBackPattern);

            MovePatternsPtr custom_moves_left_shift = std::make_shared<MovePatterns>();
            custom_moves_left_shift->reserve(20);         
            custom_moves_left_shift->push_back(kLeftTranslationPattern);
            custom_moves_left_shift->push_back(kLeftTransReducePattern);

            MovePatternsPtr custom_moves_right_shift = std::make_shared<MovePatterns>();
            custom_moves_right_shift->reserve(20);         
            custom_moves_right_shift->push_back(kRightTranslationPattern);
            custom_moves_right_shift->push_back(kRightTransReducePattern);

            /*********给更改过导航点的cell添加运动模式********/
            auto is_theta_valid = [](CorrectCellInfo cci,int theta)->bool{
                if( cci.theta & (1<<theta) )
                    return true;
                else
                    return false;
            };

            int search_theta_num = tr_cfg.search_theta_num;

            for(auto cci_x : ccs){
                for(auto cci : cci_x){

                    for(int i=0;i<size_th_;i++){
                    
                        if( !is_theta_valid(cci,i) ){
                            continue;
                        }
                        custom_move_patterns->addCustomMove(cci.cell_x, cci.cell_y, i, kMoveForwardState, custom_moves);   //前进时的可允许运动模式
                        custom_move_patterns->addCustomMove(cci.cell_x, cci.cell_y, i, kLeftMoveShiftState, custom_moves_left_shift);   //平移时的可允许运动模式
                        custom_move_patterns->addCustomMove(cci.cell_x, cci.cell_y, i, kRightMoveShiftState, custom_moves_right_shift);   //平移时的可允许运动模式
                        custom_move_patterns->addCustomMove(cci.cell_x, cci.cell_y, i, kMoveBackwardState, custom_backward_moves);   //后退时的可允许运动模式
                        
                        
                        MovePatternsPtr custom_moves_static = std::make_shared<MovePatterns>();
                        custom_moves_static->reserve(20);
                        custom_moves_static->push_back(kAcceleratePattern);
                        custom_moves_static->push_back(kReduceSpeedPattern);
                        custom_moves_static->push_back(kLeftTransAccePattern);
                        custom_moves_static->push_back(kRightTransAccePattern);
                        custom_moves_static->push_back(kLeftTransReducePattern);
                        custom_moves_static->push_back(kRightTransReducePattern);
                        custom_moves_static->push_back(kWaitPattern);
                        custom_moves_static->push_back(kTurnLeftPattern);
                        custom_moves_static->push_back(kTurnRightPattern);
                        custom_moves_static->push_back(kAccelerateBackPattern);
                        custom_moves_static->push_back(kReduceSpeedBackPattern);

                        //添加当前朝向的可允许旋转方向,旋转是连续的,如果相邻的位置不可旋转,后续的位置一样不可旋转
        
                        if( is_theta_valid(cci,(i+1)%size_th_) ){
                            custom_moves_static->push_back(kRotateLeft45Pattern);
                            if( is_theta_valid(cci,(i+2)%size_th_) ){
                                custom_moves_static->push_back(kRotateLeft90Pattern);
                                if( is_theta_valid(cci,(i+3)%size_th_) ){
                                    custom_moves_static->push_back(kRotateLeft135Pattern);
                                    if( is_theta_valid(cci,(i+4)%size_th_) )
                                        custom_moves_static->push_back(kRotate180Pattern);
                                }
                            }
                        }
                        if( is_theta_valid(cci,(i-1+size_th_)%size_th_) ){
                            custom_moves_static->push_back(kRotateR45Pattern);
                            if( is_theta_valid(cci,(i-2+size_th_)%size_th_) ){
                                custom_moves_static->push_back(kRotateR90Pattern);
                                if( is_theta_valid(cci,(i-3+size_th_)%size_th_) )
                                    custom_moves_static->push_back(kRotateR135Pattern);
                            }
                        }
                        custom_move_patterns->addCustomMove(cci.cell_x, cci.cell_y, i, kStaticState, custom_moves_static);   //静止时的可允许运动模式

                        // if(cci.cell_x == 8 && cci.cell_y == 10 && i == 2){
                        //     for(auto item : *custom_moves_static){
                        //         printf("======8 10 2 move pattern type:%d\n",item);
                        //     }
                        // }

                    }


                }
            }

        }


        findOneWayRoad();

        return true;
    }

    const CorrectCells* MoyingMap::corrected_cells(double length,double width){
        if(width <= 0 || length <= 0){
            printf("nullptr 34\n");
            return nullptr;
        }

        for(int i=0;i<corrected_cells_in_size_.size();i++){
            auto* item = &corrected_cells_in_size_[i]; 
            if( fabs(item->vehicle_length - length) <= 0.001 && fabs(item->vehicle_width - width) <= 0.001){
                auto ret = &item->cells;
                return ret;
            }
        }
        return nullptr;
    }

    const CorrectCells* MoyingMap::corrected_cells(VehiclePtr vehicle){

        if(vehicle == nullptr){
            printf("nullptr 12\n");
            return nullptr;
        }
        return corrected_cells(vehicle->getLength(),vehicle->getWidth());

    }

    const CorrectCells* MoyingMap::corrected_cells(unsigned int vehicle_id){

        return corrected_cells(getVehicle(vehicle_id));

    }
    
    CustomMovePatternsPtr MoyingMap::get_custom_move_patterns(VehiclePtr vehicle){
        if(vehicle == nullptr)
            return nullptr;

        for(int i=0;i<corrected_cells_in_size_.size();i++){
            auto* item = &corrected_cells_in_size_[i]; 
            if(EQUAL_FLOAT(item->vehicle_length,vehicle->getLength()) &&  EQUAL_FLOAT(item->vehicle_width,vehicle->getWidth()) ){
                return item->custom_move_patterns;
            }
        }
        return nullptr;
    }

    bool MoyingMap::isCellCanRotate(int mx,int my,int theta1,int theta2,double length,double width){

        if(theta1 == theta2)
            return true;

        if(corrected_cells_in_size_.empty())
            return false;

        auto* ccs = &corrected_cells_in_size_[0].cells;   
        if( !(*ccs)[mx][my].can_rotate )   //这里的标志是在寻找单行道时设置的
            return false;

        MovePatternsPtr mps = nullptr;
        for(int i=0;i<corrected_cells_in_size_.size();i++){
            auto* item = &corrected_cells_in_size_[i]; 
            if(EQUAL_FLOAT(item->vehicle_length,length) && EQUAL_FLOAT(item->vehicle_width,width) ){
                mps = item->custom_move_patterns->getCustomMove(mx,my,theta1,kStaticState);
                break;
            }
        }
        if(mps == nullptr)
            return false;
        for(auto mp_type : *mps){
            auto mp = Vehicle::getMovePattern(mp_type);
            if( (mp.dir_change+theta1+size_th_)%size_th_ == theta2 )
                return true;
        }
        return false;

    }

    bool MoyingMap::initializeTimedMap(double T, double dt)
    {
        if(!costmap_)
        {
            ERROR("Costmap has not been set yet.")
            return false;
        }

        if(dt <= 0)
            return false;

        std::lock_guard<std::recursive_mutex> lock(map_mutex_);

        time_resolution_ = dt;
        time_counts_ = ceil(T/dt);

        timed_map_.clear();
        robots_occupation_.clear();
        timed_map_.resize(size_x_);
        robots_occupation_.resize(size_x_);
        for(int x = 0; x < size_x_; x++)
        {
            timed_map_[x].resize(size_y_);
            robots_occupation_[x].resize(size_y_);
            for(int y = 0; y < size_y_; y++)
            {

                timed_map_[x][y] = nullptr;
                if(costmap_->getCost(x, y) == MAPCOSTS::FREE)
                {
                    timed_map_[x][y] = std::make_shared<std::deque<MapObjectPtr>>(time_counts_);
                }
            }
        }
        timed_map_ptr_ = std::make_shared<TimedMap>(timed_map_);        
        HIGHLIGHT("Initialized time map with time duration "<<T<<"s and step "<<dt<<"s, resulting a timed map with size "<<time_counts_);
        return true;
    }

    TimedMapPtr MoyingMap::getTimedMap()
    {   
        return timed_map_ptr_;
    }


    void MoyingMap::findOneWayRoad(){
        
        /****!!!!!!!!!!!!!!!!!这里找出的单行道,以机器人列表中第一个机器人尺寸为判断标准*****************/

        /*
        算法思路:
        1. 循环遍历所有cell,确定每一个cell是否符合单向道中cell定义.
        2. 如果是单向道cell,检查9宫格中是否已有单向道cell,如果有且和当前cell朝向一致加入已有单向道,如果不一致,则建立一个新的单向道.
        3. 检查所有单向道,删除不符合条件的单向道(cell数量太少,长度太短),剩下的就是找到的单向道
        */
        if(corrected_cells_in_size_.empty()){
            WARNING("corrected cells is empty, can't find one way road!");
            return;
        }
        all_one_way_road_.clear();

        std::vector<OneWayRoadPtr> one_way_road_temp; //临时存储所有的单向道

        std::pair<int,int> offset[] = {{0,1},{-1,1},{-1,0},{-1,-1}};
        // auto raw_cost_map = this->getCostMap()->get_raw_cost_map();
        auto cost_map = this->getCostMap()->get_cost_map();
        float max_road_width = sch::ScheduleConfig::instance()->getConfigInfo().traffic_manager_cfg.max_one_way_road_width;   //单向道最大宽度
        float res = this->getCostMap()->getResolution();
        int road_pix_size = ceil( max_road_width / res );
        printf("road_pix_size:%d\n",road_pix_size);
        // int half_cell_size = ceil( half_road_width / path_plan_param_.map_plan_resolution );

        //判断坐标点在指定的方向下是否被两侧障碍物包围,road_orientation取值0-3
        auto isHoldByObstacle = [=,this](const CorrectCells& ccs, int cell_x,int cell_y,int road_orientation)->bool{

            if(road_orientation < 0 || road_orientation > 3)
                return false;
            bool left_edge_exist = false;
            bool right_edge_exist = false;
            float left_edge_dis = 0;
            float right_edge_dis = 0;

            int x = cell_x;
            int y = cell_y;
            int max_x = this->getCostMap()->getSizeInCellsX();
            int max_y = this->getCostMap()->getSizeInCellsY();
            for(int i=0;i<road_pix_size;i++){
                x += offset[road_orientation].first;
                y += offset[road_orientation].second;
                if(x < 0 || x >= max_x || y < 0 || y >= max_y)
                    continue; 
                if( (*cost_map)[x][y] == MAPCOSTS::OBSTACLE ){
                    left_edge_dis = (i+1)*res;
                    left_edge_exist = true;
                    break;
                }
                if(ccs[x][y].cell_x == -1 || ccs[x][y].theta == 0){
                    left_edge_exist = true;
                    left_edge_dis = (i+1)*res;
                    break;
                }
            }

            //检测是否有右边界
            x = cell_x;
            y = cell_y;
            for(int i=0;i<road_pix_size;i++){
                x += -offset[road_orientation].first;
                y += -offset[road_orientation].second;
                if(x < 0 || x >= max_x || y < 0 || y >= max_y)
                    continue;
                
                if( (*cost_map)[x][y] == MAPCOSTS::OBSTACLE ){
                    right_edge_dis = (i+1)*res;
                    right_edge_exist = true;
                    break;
                }
                if(ccs[x][y].cell_x == -1 || ccs[x][y].theta == 0){
                    right_edge_exist = true;
                    right_edge_dis = (i+1)*res;
                    break;
                }
            }

            bool hold = left_edge_exist && right_edge_exist && (left_edge_dis+right_edge_dis)<=max_road_width;
            return hold;
        };
        

        unsigned char th1 = 1<<0 | 1<<4;
        // unsigned char th2 = 1<<1 | 1<<5;
        unsigned char th3 = 1<<2 | 1<<6;
        // unsigned char th4 = 1<<3 | 1<<7;
        int ori = 0;
        auto& ccs = corrected_cells_in_size_[0].cells; 
        float robot_length = corrected_cells_in_size_[0].vehicle_length;
        float robot_width = corrected_cells_in_size_[0].vehicle_width;

        for(auto& cc_in_x : ccs){    //for1
            for(auto& cci : cc_in_x){//for2

                unsigned char theta = cci.theta;
                if(theta == 0)
                    continue;
                bool only_two_direction=true;
                //检测cell是否只有两个相反的方向
                if( !(theta ^ th1 ) )
                    ori = 0;
                // else if( !(theta ^ th2 ) )
                //     ori = 1;
                else if( !(theta ^ th3 ) )
                    ori = 2;
                // else if( !(theta ^ th4 ) )
                //     ori = 3;
                else{
                    // continue;
                    only_two_direction = false;
                }
                
                if(only_two_direction){
                    //检测cell通行方向是否两侧被障碍物包围 
                    if( !isHoldByObstacle(ccs,cci.cell_x,cci.cell_y,ori) ){
                        continue;
                    }
                }else{
                    bool hold_by_obs = false;
                    if(theta | th1){
                        if( isHoldByObstacle(ccs,cci.cell_x,cci.cell_y,0) ){
                            hold_by_obs = true;
                            ori = 0;
                        }
                    }

                    if( !hold_by_obs && (theta | th3) ){
                        if( isHoldByObstacle(ccs,cci.cell_x,cci.cell_y,2) ){
                            hold_by_obs = true;
                            ori = 2;
                        }
                    }

                    // if( !hold_by_obs && (theta | th2) ){
                    //     if( isHoldByObstacle(ccs,cci.cell_x,cci.cell_y,1) ){
                    //         hold_by_obs = true;
                    //         ori = 1;
                    //     }
                    // }

                    // if( !hold_by_obs && (theta | th4) ){
                    //     if( isHoldByObstacle(ccs,cci.cell_x,cci.cell_y,3) ){
                    //         hold_by_obs = true;
                    //         ori = 3;
                    //     }
                    // }
                    if(!hold_by_obs)
                        continue;

                }

                //如果9宫格中有cell在单向道中,且当前cell的朝向一致,加入到已有单向道中
                int minx=cci.cell_x-1;
                int maxx=cci.cell_x+1;
                int miny=cci.cell_y-1;
                int maxy=cci.cell_y+1;
 
                auto cell = MapCell(cci.cell_x,cci.cell_y);
                bool add_exist_road = false;
                OneWayRoadPtr added_road = nullptr;
                for(int x=minx;x<=maxx;x++){
                    if(x<0||x>=size_x_)
                        continue;
                    for(int y=miny;y<=maxy;y++){
                        if(y<0||y>=size_y_)
                            continue;
                        if(x == cci.cell_x && y == cci.cell_y)
                            continue;
                        for(auto owr : one_way_road_temp){
                            if( owr->isCellInRoad(MapCell(x,y)) && owr->isAvailabeOrientation(Orientation(ori)) ){
                                if( !add_exist_road ){
                                    owr->addCell(cell);
                                    added_road = owr;
                                    cci.can_rotate = false;
                                    add_exist_road = true;
                                }else{
                                    added_road->mergeOneWayRoad(owr);
                                }
                                break;
                            }
                        }
                        // if(add_exist_road)
                        //     break;
                    }
                    // if(add_exist_road)
                    //     break;
                }

                //添加新的单向道
                if(!add_exist_road){
                    OneWayRoadPtr owr_ptr = std::make_shared<OneWayRoad>();
                    std::vector<Orientation> orientations;
                    orientations.emplace_back(Orientation(ori));
                    orientations.emplace_back(Orientation(ori+4));
                    owr_ptr->set_available_orientation(orientations);
                    owr_ptr->addCell(cell);
                    one_way_road_temp.emplace_back(owr_ptr);
                    cci.can_rotate = false;
                }
            }//for2
        }//for1

        //删除不符合条件的单向道
        int min_road_len = sch::ScheduleConfig::instance()->getConfigInfo().traffic_manager_cfg.min_one_way_road_length;
        printf("one_way_road_temp size:%d\n",one_way_road_temp.size());
        for(auto owr_ptr : one_way_road_temp){
            if(owr_ptr->roadLength()*res < min_road_len)
                continue;
            all_one_way_road_.emplace_back(owr_ptr);
            owr_ptr->printfInfo();
        }
        printf("find %d one way road totally!\n",all_one_way_road_.size());

    }

    bool MoyingMap::isFootprintFreeInRecentSeconds(const FootprintOnMap& footprint,int seconds,int except_robot_id){

        int t = seconds;
        if(time_resolution_ != 0) 
            t = ceil(seconds/time_resolution_);
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        for(auto cell : footprint){
            TimeOccupationListPtr ptr = timed_map_[cell.second.x][cell.second.y];
            if(ptr)
            {
                for(int i=0;i<t;i++){
                    auto obj = ptr->at(i);
                    if(obj != nullptr){
                        if(except_robot_id != 0){
                            VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(obj);
                            VehiclePtr v = getVehicle(except_robot_id);
                            if(v != nullptr){
                                if(vehicle_inflation != nullptr && vehicle_inflation->canOccupy(v)){
                                    return true;
                                }
                            }
                            
                        }
                        
                        return false;
                    }
                }
                
            } 
        }
        return true;

    }

    bool MoyingMap::isVehicleFreeLocateCellInRecentSeconds(unsigned int vehilce_id,double wx,double wy,int seconds){
        
        auto vehicle = getVehicle(vehilce_id);
        if(vehicle == nullptr)
            return false;
        int mx,my;
        if( !costmap_->worldToMap(wx,wy,mx,my) )
            return false;
        FootprintOnMap fpom;
        Node node;
        node.x = mx;
        node.y = my;
        if( !getFootprintCell(vehicle,node,fpom) )
            return false;
        
        return isFootprintFreeInRecentSeconds(fpom,seconds,vehilce_id);
    }

    bool MoyingMap::isPoseInOneWayRoad(float x,float y,unsigned int& road_id,std::vector<unsigned int>& vehicle_ids){

        MapCell cell;
        if( !costmap_->worldToMap(x,y,cell.x,cell.y) )
            return false;

        for(auto owr : all_one_way_road_){
            if( owr->isCellInRoad(cell) ){
                road_id = owr->id();
                owr->getAllVehicles(vehicle_ids);
                return true;
            }
        }

        return false;
    }

    void MoyingMap::fromIndex(int index,int& x, int& y, int& th, int& t, int& sm)
    {
        x = index % size_x_;
        index /=  size_x_;
        y = index  % size_y_;
        index /=  size_y_;
        th = index  % size_th_;
        index /=  size_th_;
        if(size_sm_ > 0){
            sm = index % size_sm_;
            index /= size_sm_;
            t = index;
        }else{
            t = index;
        }
        return ;
    }

    bool MoyingMap::set_path_planing_vehicle_id(unsigned int vehicle_id){

        VehiclePtr v_ptr = getVehicle(vehicle_id);
        if(v_ptr == nullptr){
            path_planing_vehicle_id_ = 0;
            path_planing_vehicle_ptr_ = nullptr;
            return false;
        }else{
            path_planing_vehicle_id_ = vehicle_id;
            path_planing_vehicle_ptr_ = v_ptr;
            // path_planing_obs_info_ = local_obstacle_[vehicle_id];
            path_planing_obs_info_ = v_ptr->obstacle_info();
            return true;
        }
        
    }

    VehiclePtr MoyingMap::getMinDisVehicle(VehiclePtr vehicle, float& min_dis ){
        if(vehicle == nullptr)
            return nullptr;
        min_dis = INT32_MAX;
        VehiclePtr min_dis_vehicle = nullptr;

        VehicleState state,state1;
        vehicle->getPose(state);
        for(auto item : vehicles_){
            if(item.second->getID() == vehicle->getID())
                continue;
            VehiclePtr veh = item.second;
            veh->getPose(state1);
            float x_dis = fabs(state.x - state1.x);
            float y_dis = fabs(state.y - state1.y);
            float dis = sqrt(x_dis*x_dis+y_dis*y_dis);

            if(dis < min_dis){
                min_dis = dis;
                min_dis_vehicle = veh;
            }
        }
        return min_dis_vehicle;
    }


    bool MoyingMap::isFree(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb)
    {
        // return true;
        // if(addon_costmap_[sa->x][sa->y] != nullptr && addon_costmap_[sa->x][sa->y] != vehicle)
        //     return false;
        // if(addon_costmap_[sb->x][sb->y] != nullptr && addon_costmap_[sb->x][sb->y] != vehicle)
        //     return false;

    
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        TimeOccupationListPtr list_ptr_a = timed_map_[sa->x][sa->y];
        if(list_ptr_a == nullptr)
            return false;

        TimeOccupationListPtr list_ptr_b = timed_map_[sb->x][sb->y];
        if(list_ptr_b == nullptr)
            return false;
        
        for(int t = sa->t;t<=sb->t;t++){
            if(list_ptr_a->at(t) != nullptr){
                // WARNING("isFree false x:"<<sb->x<<" y:"<<sb->y<<" t:"<<sb->t);
                if(list_ptr_a->at(t)->getName().compare("VehicleInflation") == 0)
                {
                    VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr_a->at(t));
                    if(vehicle_inflation == nullptr || !vehicle_inflation->canOccupy(vehicle))
                    {
                        return false;
                    }
                    
                }else{
                    WARNING("time cell is not inflation,x:"<<sa->x<<" y:"<<sa->y);
                    return false;
                }
            }

            if(list_ptr_b->at(t) != nullptr){
                if(list_ptr_b->at(t)->getName().compare("VehicleInflation") == 0)
                {
                    VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr_b->at(t));
                    if(vehicle_inflation == nullptr || !vehicle_inflation->canOccupy(vehicle))
                    {
                        return false;
                    }
                    
                }else
                {
                    WARNING("time cell is not inflation,x:"<<sb->x<<" y:"<<sb->y);
                    return false;
                }

            }
        }

        return true;
    }
    
    bool MoyingMap::occupy(const VehiclePtr &vehicle, const Node &state)
    {
        return occupy(vehicle, state.x, state.y, state.t);
    }

    bool MoyingMap::occupy(const VehiclePtr &vehicle, const NodePtr &state)
    {
        return occupy(vehicle, state->x, state->y, state->t);
    }

    bool MoyingMap::occupy(const VehiclePtr &vehicle, int x, int y, int t)
    {
        std::map<int, FootprintCell> footprint_cells;
        if(!vehicle->getFootprintOccupiedCells(costmap_, x, y, footprint_cells))
            return false;
        
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        TimeOccupationListPtr list_ptr;
        for(auto &it: footprint_cells)
        {
            list_ptr = timed_map_[it.second.x][it.second.y];
            if(list_ptr != nullptr)
                list_ptr->at(t) = vehicle;
        }
    }
    
    bool MoyingMap::occupy(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::map<int, std::vector<FootprintCell>> &occupied_ceels)
    {
        return occupy(vehicle, sa.x, sa.y, sa.t, sb.x, sb.y, sb.t, occupied_ceels);
    }

    bool MoyingMap::occupy(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb, std::map<int, std::vector<FootprintCell>> &occupied_ceels)
    {
        return occupy(vehicle, sa->x, sa->y, sa->t, sb->x, sb->y, sb->t, occupied_ceels);
    }

    bool MoyingMap::occupy(const VehiclePtr &vehicle, int xa, int ya, int ta, int sb, int yb, int tb, std::map<int, std::vector<FootprintCell>> &occupied_ceels)
    {
        std::lock_guard<std::recursive_mutex> lk(map_mutex_);
        std::map<int, FootprintCell> footprint_cells;
        if(!vehicle->getFootprintOccupiedCells(costmap_, xa, ya, footprint_cells))
            return false;
        if(!vehicle->getFootprintOccupiedCells(costmap_, sb, yb, footprint_cells))
            return false;
        
        TimeOccupationListPtr list_ptr;
        //这里在原来时刻前后多占用5个时刻,可以弥补机器人导航到达时间不精准的问题,而且也可以加达机器人路径间的距离
        int start_t = ta-5;
        start_t = start_t < 0 ? 0 : start_t;
        for(int t = start_t; t<=tb+5; t++)
        {
            if(t >= time_counts_){
                WARNING("time is exceed limit, t:"<<t<<" ta:"<<ta<<" tb:"<<tb);
                return false;
            }
            for(auto &it: footprint_cells)
            {
                list_ptr = timed_map_[it.second.x][it.second.y];
                if(list_ptr != nullptr)
                {
                    if(list_ptr->at(t) == nullptr)
                    {
                        VehicleInflationPtr vehicle_inflation = std::make_shared<VehicleInflation>();
                        vehicle_inflation->registerVehicle(vehicle);
                        list_ptr->at(t) = vehicle_inflation;
                        occupied_ceels[t].push_back(it.second);
                    }
                    else if(list_ptr->at(t)->getType() == "VehicleInflation")
                    {
                        VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr->at(t));
                        vehicle_inflation->registerVehicle(vehicle);
                        occupied_ceels[t].push_back(it.second);
                    }
                    else if(!it.second.inflate)
                    {
                        if(list_ptr->at(t) != vehicle)
                            HIGHLIGHT("Invalid time occupation: "<<vehicle->getName()<<" wants to occupy a cell that is currently occupied by "<<list_ptr->at(t)->getName()<<" at "<<it.second.x<<" "<<it.second.y<<" "<<t);
                    }
                }
            }
        }
        return true;
    }

    
    void MoyingMap::occupyTimeMap(const VehiclePtr vehicle, PlanningResult &result)
    {
        if(result.node_path.size() > 0)
        {
            
            result.timed_footprints.clear();

            for(int i = 1; i < result.node_path.size(); i++)
            {
                if(i == 0){
                }else{  
                    occupy(vehicle, result.node_path[i - 1], result.node_path[i], result.timed_footprints);
                }
            }

            if(result.occupy_time_after_arrive > 0 && time_resolution_ != 0){
                int times = round(result.occupy_time_after_arrive / time_resolution_);
                int t1,t2;
                t1 = result.node_path.back().t;
                t2 = t1+1;
                int x = result.node_path.back().x;
                int y = result.node_path.back().y;
                for(int i=0;i<times;i++){
                    occupy(vehicle,x,y,t1,x,y,t2,result.timed_footprints);
                    t1++;
                    t2++;
                }
            }
        }

        printf("!!!! occupy footprint size:%d robot_id:%d\n",result.timed_footprints.size(),vehicle->getID());
    }

    bool MoyingMap::occupyTimeMap(unsigned int robot_id,unsigned int occupy_seconds){

        VehiclePtr vehicle = getVehicle(robot_id);
        if(vehicle == nullptr){
            return false;
        }

        int t1 = 0;
        int t2 = 1;
        int mx,my;
        costmap_->worldToMap(vehicle->getPose().x,vehicle->getPose().y,mx,my);
        TimedFootprints& occupied_footprint_cells = vehicle->temp_time_occupy();

        int occupy_times = time_resolution_ != 0 ? occupy_seconds / time_resolution_ : occupy_seconds;
        for(int i=0;i<occupy_times;i++){
            occupy(vehicle, mx, my,t1++,mx,my,t2++, occupied_footprint_cells);
        }
        INFO("occupy timemap robot_id:"<<robot_id<<" seconds:"<<occupy_seconds);
        return true;
    }


    void MoyingMap::updateTimeMap(const std::chrono::system_clock::time_point &current_time,int update_times)
    {
        std::lock_guard<std::recursive_mutex> lk(map_mutex_);
        current_time_ = current_time;
        for(int x = 0; x < size_x_; x++)
        {
            for(int y = 0; y < size_y_; y++)
            {
                
                TimeOccupationListPtr ptr = timed_map_[x][y];
                if(ptr)
                {
                    for(int i=0;i<update_times;i++){
                        ptr->pop_front();
                        ptr->push_back(nullptr);
                    }
                    
                }
            }
        }
    }

    void MoyingMap::updateRobotPoseToTimedMap(const VehiclePtr &vehicle)
    {
    }

    std::chrono::system_clock::time_point MoyingMap::getNextPlanningTime()
    {
        // unsigned int time_left = std::chrono::duration_cast<std::chrono::milliseconds>(next_update_time_ - std::chrono::system_clock::now()).count();
        // if(time_left > 800)
        //     return next_update_time_ - std::chrono::milliseconds(1000);
        // else
        //     return next_update_time_;
    }
  
    bool MoyingMap::clearVehicleFootprint(const VehiclePtr &vehicle, TimedFootprints &footprint){

        boost::dynamic_bitset checked;
        int size = size_x_*size_y_;
        checked.resize(size,false);

        if(footprint.empty())
            return true;
            
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        for(auto it: footprint)
        {//for1
                
            //如果路径中有比当前时间滞后的点,清空时间占用列表对应的信息
            for(int i = 0; i < it.second.size(); i++)
            {//for2
                int x = it.second[i].x;
                int y = it.second[i].y;

                int index =x+y*size_x_ ;
                if(index >= size || index < 0 ){
                    continue;
                }

                if(checked[index])
                    continue;

                checked[x+y*size_x_] = true;
                TimeOccupationListPtr list_ptr = timed_map_[x][y];
                if(list_ptr != nullptr)
                {//if1
                    int n = list_ptr->size();
                    for(int t=0;t<list_ptr->size();t++){
                        if(list_ptr->at(t) == nullptr)
                        {
                            continue;
                        }
                        if(list_ptr->at(t) == vehicle)
                            list_ptr->at(t).reset();
                        else if(list_ptr->at(t)->getName().compare("VehicleInflation") == 0)
                        {
                            VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr->at(t));
                            if(vehicle_inflation == nullptr)
                                continue;
                            // vehicle_inflation->unregisterVehicle(vehicle);
                            if(vehicle_inflation->hasUniqueVehicle(vehicle))
                            {
                                list_ptr->at(t).reset();
                            }
                            else
                            {
                                vehicle_inflation->unregisterVehicle(vehicle);
                            }
                        }
                    }
                }//if1
            }//for2
        }//for1
        footprint.clear();
        return true;
    }

    bool MoyingMap::clearVehiclePath( VehiclePtr &vehicle)
    {
        if(vehicle == nullptr)
            return false;
        
        std::lock_guard<std::recursive_mutex> lk(map_mutex_);

        PlanningResult &result = vehicle->path_plan_result();
        int t_diff = std::chrono::duration_cast<std::chrono::seconds>(current_time_ - result.start_time).count();
        int t, count = 0;

        printf("!!!! clear footprint size:%d robot_is:%d\n",result.timed_footprints.size(),vehicle->getID());

        clearVehicleFootprint(vehicle,result.timed_footprints);
        clearVehicleFootprint(vehicle,vehicle->temp_time_occupy());
        result.clear();
        return true;

    }
    

    bool MoyingMap::clearVehiclePath(unsigned int id)
    {
        VehiclePtr vehicle = getVehicle(id);
        if(vehicle == nullptr)
            return false;
        // PlanningResult &result = vehicle->path_plan_result();
        return clearVehiclePath(vehicle);
    }

    bool MoyingMap::isFree(const OccupiedSpace &occup)
    {
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        for(int i = 0; i < occup.size(); i++)
        {
            TimeOccupationListPtr list_ptr = timed_map_[occup[i].x][occup[i].y];
            if(list_ptr == nullptr)
                continue;
            for(int t = 0; t < time_counts_; t++)
            {
                if(list_ptr->at(t) != nullptr)
                {
                    return false;
                }
            }
        }
        return true;
    }


    bool MoyingMap::hasVehicle(unsigned int id)
    {
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        return vehicles_.find(id) != vehicles_.end();
    }

    bool MoyingMap::addVehicle(unsigned int id, std::string type, const std::string &name,moying_proto::NavBaseType nav_base_type)
    {
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        if(vehicles_.find(id) == vehicles_.end())
        {
            /*TODO 根据不同的类型,添加不同的机器人实际对象*/
            VehiclePtr vehicle_ptr = nullptr;
            switch(nav_base_type){
                case moying_proto::NavBaseType::kOmniBaseType:
                    vehicle_ptr = std::make_shared<navigation::MecanumVehicle>(type, name, id);
                    vehicles_.emplace(id, vehicle_ptr);
                break;
                case moying_proto::NavBaseType::kDiffBaseType:
                    vehicle_ptr = std::make_shared<navigation::DiffDriveVehicle>(type, name, id);
                    vehicles_.emplace(id, vehicle_ptr);
                break;
                default:
                break;
            }
            
            vehicles_[id]->setMovePatterns();
            return true;
        }
        else
        {
            HIGHLIGHT("Robot "<<id<<" already exist");
            return false;
        }
    }

    VehiclePtr MoyingMap::getVehicle(unsigned int id)
    {
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        if(vehicles_.find(id) != vehicles_.end())
            return vehicles_[id];
        else
            return nullptr;
    }

    bool MoyingMap::deleteVehicle(unsigned int id)
    {

        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        if(vehicles_.find(id) != vehicles_.end())
        {
            vehicles_.erase(id);
            // local_costmap_.erase(id);
            // local_obstacle_.erase(id);
            vehicle_footprints_.erase(id);
            return true;
        }
        return false;
    }

    bool MoyingMap::isOnMap(int x, int y, int th)
    {
        return x >= 0 && x < size_x_ && y >= 0 && y < size_y_ && th >= 0 && th < size_th_; 
    }

    bool MoyingMap::isOnMap(int x, int y, int th, int t)
    {
        return x >= 0 && x < size_x_ && y >= 0 && y < size_y_ && th >= 0 && th < size_th_ && t >= 0 && t < time_counts_;
    }

    void MoyingMap::addTimeList(int x, int y)
    {
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        auto time_queue = std::make_shared<std::deque<MapObjectPtr>>(time_counts_);
        timed_map_[x][y] = time_queue;

    }

    bool MoyingMap::updateVehiclePose(unsigned int id, double x, double y, double th)
    {
        VehiclePtr vehicle_ptr = getVehicle(id);
        if(vehicle_ptr == nullptr)
            return false;
        vehicle_ptr->updatePose(x, y, th);
        vehicle_ptr->updateCurFootprint(costmap_);
        //清除残留footprint
        for(auto &it: vehicle_footprints_[id])
        {
            robots_occupation_[it.second.x][it.second.y].reset();
        }

        vehicle_footprints_[id].clear();
        vehicle_ptr->getFootprintOccupiedCells(costmap_, vehicle_ptr->getPose(), vehicle_footprints_[id]);
        for(auto &it: vehicle_footprints_[id])
        {
            robots_occupation_[it.second.x][it.second.y] = vehicle_ptr;
        }

        int mx =0;
        int my =0;
        if( costmap_->worldToMap(x,y,mx,my) ){ 
            for(auto owr : all_one_way_road_){
                auto ccs = corrected_cells(vehicle_ptr);
                if( (*ccs)[mx][my].cell_x != -1 && (*ccs)[mx][my].theta != 0 ){ //如果机器人当前坐标是一个无效的cell不处理
                    owr->handleVehiclePoseUpdate( vehicle_ptr,MapCell(mx,my) );
                }
            }
        }
        return true;
    }


    bool MoyingMap::isTimeValid(const VehiclePtr &vehicle, int x, int y, int t)
    {
        if(t<0 || t>= time_counts_)
        {
            
            ERROR("time value is out of [0, 599]!");
            return false;
        }

        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        if(timed_map_[x][y] == nullptr)
        {
            ERROR("This grid is obstacle, every time is not valid!")
            return false;
        }
        else
        {
            if(timed_map_[x][y]->at(t) != vehicle && timed_map_[x][y]->at(t) != nullptr && timed_map_[x][y]->at(t)->getName() != "VehicleInflation")
            {
                ERROR("x: " << x << " y:" << y << " t:" << t << " has other vehicle.");
                return false;
            }
        }
        return true;
    }
    
    bool MoyingMap::isAvaiableElimateDiff(const VehiclePtr &vehicle, PlanningResult& result, int time_delay_s, int begin_index)
    {
        bool available = true;
        for(int index = begin_index; index < result.node_path.size(); ++index)
        {
            result.node_path[index].t += time_delay_s;
        }

        std::map<int, FootprintCell> footprint_cells;
        for(int index = begin_index; index < result.node_path.size() - 1; ++index)
        {
            if(getFootprintCell(vehicle, result.node_path[index], result.node_path[index+1], footprint_cells))
            {
                if(isPassable(vehicle, result.node_path[index], result.node_path[index+1], footprint_cells))
                {
                    continue;
                }
                else
                {
                    ERROR("Not passable at t:" << result.node_path[index].t << "-" << result.node_path[index+1].t);
                    available = false;
                    break;
                }    
            }
            else
            {
                ERROR("Cann't get footPrintCell.");
                available = false;
                break;
            }           
        }
        return available;

    }

    bool MoyingMap::correctTimeMap(const VehiclePtr &vehicle, PlanningResult& result, int time_delay_s, int begin_index, std::vector<Node>& new_node_path)
    {
        std::map<int, std::vector<FootprintCell>> tmp_timed_footprints;
        int path_size = result.node_path.size();

        // step1: correct PlanningResult
        for(int i = begin_index; i < path_size; ++i)
        {
            if(time_delay_s > 0)
            {
                result.path[i].t += std::chrono::milliseconds(1000 * time_delay_s);
            }
            else
            {
                result.path[i].t -= std::chrono::milliseconds(1000 * time_delay_s);
            }
        }

        result.goal = result.path[result.path.size() - 1];
        result.goal_time = result.goal.t;

        HIGHLIGHT("new_node_path size "<<new_node_path.size()<<" last t:" << new_node_path[new_node_path.size()-1].t)
        // step1.2 fill timed_footprints
        for(int i = 0; i < new_node_path.size(); ++i)
        {
            HIGHLIGHT(i<<" "<<new_node_path[i].x<<" "<<new_node_path[i].y<<" "<<new_node_path[i].t);
            occupy(vehicle, new_node_path[i], new_node_path[i+1], tmp_timed_footprints);
        }
        result.timed_footprints = tmp_timed_footprints;
        HIGHLIGHT("XXXXXXXXXXXXX");
  
        return true;
    }

    bool MoyingMap::isPassable(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::map<int, FootprintCell> &footprint_cells)
    {
        for(int t = sa.t; t<=sb.t; t++)
        {
            if(!isPassable(vehicle, t, footprint_cells))
                return false;
        }
    }

    bool MoyingMap::isPassable(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb, std::map<int, FootprintCell> &footprint_cells)
    {
        for(int t = sa->t; t<=sb->t; t++)
        {
            if(!isPassable(vehicle, t, footprint_cells))
                return false;
        }
        return true;
    }

    bool MoyingMap::isPassable(const VehiclePtr &vehicle, short t, std::map<int, FootprintCell> &footprint_cells)
    {
        // std::map<int, FootprintCell> footprint_cells;
        // if(!vehicle->getFootprintOccupiedCells(costmap_, st, footprint_cells))
        //     return false;

        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        bool is_success = true;
        TimeOccupationListPtr list_ptr;
        for(auto &it: footprint_cells)
        {
            if(it.second.inflate == true)
            {
                continue;
            }
            else
            {
                list_ptr = timed_map_[it.second.x][it.second.y];
                if(list_ptr != nullptr) //不是障碍物
                {
                    if(list_ptr->at(t) == nullptr || list_ptr->at(t) == vehicle || list_ptr->at(t)->getType() == "VehicleInflation")
                    {
                        continue;
                    }
                    is_success = false;
                    break;
                }
            }
        }

        return is_success;
    }

    bool MoyingMap::getFootprintCell(const VehiclePtr &vehicle, std::map<int, FootprintCell> & footprint_cells){
        return vehicle->getFootprintOccupiedCells(costmap_, footprint_cells);
    }

    bool MoyingMap::getFootprintCell(const VehiclePtr &vehicle, const Node &st, std::map<int, FootprintCell> & footprint_cells)
    {
        return vehicle->getFootprintOccupiedCells(costmap_, st.x, st.y, footprint_cells);
    }
    bool MoyingMap::getFootprintCell(const VehiclePtr &vehicle, const NodePtr &st, std::map<int, FootprintCell> &footprint_cells)
    {
        return vehicle->getFootprintOccupiedCells(costmap_, st, footprint_cells);
    }

    bool MoyingMap::getFootprintCell(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::map<int, FootprintCell> &footprint_cells)
    {
        if(!vehicle->getFootprintOccupiedCells(costmap_, sa.x, sa.y, footprint_cells))
            return false;
        if(!vehicle->getFootprintOccupiedCells(costmap_, sb.x, sb.y, footprint_cells))
            return false;
        return true;
    }

    bool MoyingMap::getFootprintCell(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb, std::map<int, FootprintCell> &footprint_cells)
    {
        if(!vehicle->getFootprintOccupiedCells(costmap_, sa, footprint_cells))
            return false;
        if(!vehicle->getFootprintOccupiedCells(costmap_, sb, footprint_cells))
            return false;
        return true;
    }

    bool MoyingMap::empty(const VehiclePtr &vehicle, const Node &st, std::vector<FootprintCell> &footprint_cells)
    {
        int size = footprint_cells.size();
        short t = st.t;
        TimeOccupationListPtr list_ptr;
        
        std::lock_guard<std::recursive_mutex> lock(map_mutex_);
        HIGHLIGHT("Empty t:" << t);
        for(auto& it: footprint_cells)
        {
            list_ptr = timed_map_[it.x][it.y];

            // obstacle
            if(list_ptr == nullptr)
            {
                //HIGHLIGHT("Empty(clear path) in obstacle");
                continue;
            }

            //not obstacle
            else
            {
                if(list_ptr->at(t) == nullptr)
                {
                    //ERROR("Impossible there is no mapobject!");
                    continue;
                }
                else if(list_ptr->at(t) == vehicle)
                {   
                    //HIGHLIGHT("Empty Vehicle");
                    list_ptr->at(t).reset();
                }
                else if(list_ptr->at(t)->getType() == "VehicleInflation")
                {
                    //HIGHLIGHT("Empty VehicleInflation");
                    VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr->at(t));
                    // vehicle_inflation->unregisterVehicle(vehicle);
                    if(vehicle_inflation->hasUniqueVehicle(vehicle))
                    {
                        list_ptr->at(t).reset();
                    }
                    else
                    {
                        vehicle_inflation->unregisterVehicle(vehicle);
                    }
                    
                }
                else
                {
                    ERROR("Logic error in empty timed_map "<<list_ptr->at(t)->getType());
                }
                
            }
        }

        HIGHLIGHT("Successly empty timed_map at time:" << t);
    }


    bool MoyingMap::empty(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::vector<FootprintCell> &footprint_cells)
    {
        
    }

    bool MoyingMap::updateLocalObstacle(unsigned int robot_id,const LocalObstacleInfo& obs_info){

        if(robot_id == path_planing_vehicle_id_)
            return false;

        if(robot_id <= 0)
            return false;

        VehiclePtr vehicle = getVehicle(robot_id);

        if(vehicle == nullptr)
            return false;

        vehicle->set_obstacle_info(obs_info);
        // std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        // local_obstacle_[robot_id] = obs_info;
        return true;    
        
    }


    bool MoyingMap::getLocalObstacle(unsigned int robot_id,LocalObstacleInfo& obs_info){
        
        VehiclePtr vehicle = getVehicle(robot_id);
        if( vehicle != nullptr ){
            obs_info = vehicle->obstacle_info();
            return true;
        }else{
            return false;
        }
    }
    
    bool MoyingMap::getRobotMinDisToObstacle(unsigned int robot_id,float& min_dis ){

        LocalObstacleInfo obs_info;
        if(getLocalObstacle(robot_id,obs_info)){
            min_dis = obs_info.min_obstacle_dis;
            return true;
        }else{
            return false;
        }

    }

    bool MoyingMap::isPointInRect(Coordinate pt,Coordinate lt,Coordinate rt,Coordinate rb,Coordinate lb,int& time){

        float min_x = fmin(fmin(lt.x,rt.x),fmin(rb.x,lb.x));
        float max_x = fmax(fmax(lt.x,rt.x),fmax(rb.x,lb.x));
        float min_y = fmin(fmin(lt.y,rt.y),fmin(rb.y,lb.y));
        float max_y = fmax(fmax(lt.y,rt.y),fmax(rb.y,lb.y));
        if(pt.x < min_x || pt.x > max_x || pt.y <= min_y || pt.y >= max_y){
            // time = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start_time).count();
            return false;
        }

        float a,b,c,d;

        a = (rt.x - lt.x) * (pt.y - lt.y) - (pt.x - lt.x) * (rt.y - lt.y);
        b = (rb.x - rt.x) * (pt.y - rt.y) - (pt.x - rt.x) * (rb.y - rt.y);
        c = (lb.x - rb.x) * (pt.y - rb.y) - (pt.x - rb.x) * (lb.y - rb.y);
        d = (lt.x - lb.x) * (pt.y - lb.y) - (pt.x - lb.x) * (lt.y - lb.y);
        bool ret = a*c >= 0 && b*d >= 0;

        // time = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start_time).count();
        return ret;
    }

    bool MoyingMap::checkCurrentPathPlanLocalCollision(int center_mx,int center_my){

        if(path_planing_vehicle_ptr_ == nullptr){
            return false;
        }

        auto& obs = path_planing_obs_info_;
        if(center_mx < obs.min_cell_x || center_mx > obs.max_cell_x || center_my < obs.min_cell_y || center_my > obs.max_cell_y){
            // WARNING("local obs can pass");
            return false;
        }
        for(auto cell : obs.occupancy_cells){
            if(cell.first == center_mx && cell.second == center_my){
                return true;
            }
        }
        return false;
    }

    bool MoyingMap::checkCollideLocalObstacleCell(unsigned int robot_id,int center_mx,int center_my,bool check_robot_occupancy){

        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        LocalObstacleInfo& obs = path_planing_obs_info_;
        VehiclePtr vehicle = nullptr;
        if(path_planing_vehicle_ptr_ != nullptr && path_planing_vehicle_ptr_->getID() == robot_id){
            obs = path_planing_obs_info_;
            vehicle = path_planing_vehicle_ptr_;
        }else{
            vehicle = vehicles_[robot_id];
            if(vehicle)
                obs = vehicle->obstacle_info();
        }

        if(center_mx < obs.min_cell_x-2 || center_mx > obs.max_cell_x+2 || center_my < obs.min_cell_y-2 || center_my > obs.max_cell_y+2)
            return false;

        
        bool collide = false;
        for(auto cell : obs.occupancy_cells){//for1
            if(!check_robot_occupancy){//if2
                if(cell.first == center_mx && cell.second == center_my){
                    collide = true;
                    break;
                }
            }else{
                if(vehicle){
                    FootprintOnMap footprint_cells;
                    vehicle->getFootprintOccupiedCells(costmap_,center_mx,center_my,footprint_cells);
                    for(auto item : footprint_cells){
                        auto c = item.second;
                        if(!item.second.inflate && cell.first == c.x && cell.second == c.y){
                            collide = true;
                            break;
                        }
                    }
                    if(collide)
                        break;
                }
                
            }//if2
            
        }//fo1

        return collide;
    }

    bool MoyingMap::checkCollideLocalObstacle(unsigned int robot_id,float center_wx,float center_wy,float theta,
            std::vector<Coordinate>* pt_vector_ptr,float l_padding,float w_padding,bool temp_obstacle,bool check_collide_with_static_map){

        if(robot_id <= 0){
            WARNING("!!! robot_id is less than zero!");
            return false;
        }

        VehiclePtr vehicle_ptr = getVehicle(robot_id);
        if(vehicle_ptr == nullptr){
            WARNING("checkCollideLocalObstacle return false , can't find vehicle"<<robot_id);
            return false;
        }

        float world_length = vehicle_ptr->getLength();
        float world_width = vehicle_ptr->getWidth();

        world_length += l_padding;
        world_width  += w_padding;

        const moying_proto::LocalObstacle* local_obstacle_ptr_ = nullptr;
        {
            if(temp_obstacle){
                local_obstacle_ptr_ = &vehicle_ptr->obstacle_info().temp_obstacle;
            }else{
                local_obstacle_ptr_ = &vehicle_ptr->obstacle_info().raw_obstacle;
            }

            if(local_obstacle_ptr_->data_size() == 0){
                return false;
            }

        }

        if(local_obstacle_ptr_ == nullptr)
            return false;

        // WARNING("check obstacle");
        float raw_res = costmap_->getRawResolution();
        float min_x = local_obstacle_ptr_->origin_x();
        float min_y = local_obstacle_ptr_->origin_y();
        float max_x = local_obstacle_ptr_->origin_x()+local_obstacle_ptr_->width() * raw_res;
        float max_y = local_obstacle_ptr_->origin_y()+local_obstacle_ptr_->height() * raw_res;
        if(center_wx < min_x-1 || center_wx > max_x+1
            || center_wy < min_y-1 || center_wy > max_y+1){
            //检测点不在局部障碍物上
            return false;
        }
        // printf("minx:%.2f,miny:%.2f,maxx:%.2f,maxy:%.2f cx:%.2f,cy:%.2f\n",min_x,min_y,max_x,max_y,center_wx,center_wy);
        // WARNING("check local obstacle");
        float width_padding = w_padding;
        float length_padding = l_padding;
        float width     = world_width + width_padding;
        float length    = world_length + length_padding;
        Coordinate lt,rt,rb,lb;

        //机器人水平朝向的矩形坐标
        lb.x = center_wx - length/2;  lb.y = center_wy - width/2;
        rt.x = center_wx + length/2;  rt.y = center_wy + width/2;
        lt.x = lb.x;    lt.y = rt.y;
        rb.x = rt.x;    rb.y = lb.y;

        // if(path_plan_check){

        // }else
        {//导航时避障用
            //机器人旋转后的矩形坐标
            float x,y;
            x = lt.x; y = lt.y;
            lt.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            lt.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

            x = rt.x; y = rt.y;
            rt.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            rt.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

            x = rb.x; y = rb.y;
            rb.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            rb.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;

            x = lb.x; y = lb.y;
            lb.x = (x-center_wx)*cos(theta) - (y-center_wy)*sin(theta) + center_wx;
            lb.y = (x-center_wx)*sin(theta) + (y-center_wy)*cos(theta) + center_wy;
        }
        
        bool collide = false;
        
        //检测障碍物数据
        if(local_obstacle_ptr_->data_size() > 0){
            // WARNING("obs size:"<<local_obstacle_ptr_->data_size());
            for(auto obstacle_pos : local_obstacle_ptr_->data()){

                auto start_time2 =  std::chrono::system_clock::now();
                Coordinate obs_coor;
                obs_coor.x = (obstacle_pos.x()+0.5)*raw_res + local_obstacle_ptr_->origin_x();
                obs_coor.y = (obstacle_pos.y()+0.5)*raw_res + local_obstacle_ptr_->origin_y();

                int time=0;
                if(isPointInRect(obs_coor,lt,rt,rb,lb,time)){
                    
                    if(pt_vector_ptr != nullptr){
                        pt_vector_ptr->emplace_back(lt);
                        pt_vector_ptr->emplace_back(rt);
                        pt_vector_ptr->emplace_back(rb);
                        pt_vector_ptr->emplace_back(lb);
                        pt_vector_ptr->emplace_back(obs_coor);
                    }
                    collide = true;
                    // printf("===== collide_x:%.2f y:%.2f\n",obs_coor.x,obs_coor.y);
                    break;
                }
            }
        }

        //检测是否和静态地图相撞
        if(check_collide_with_static_map){
            float hl = vehicle_ptr->getLength()/2;
            float min_wx = center_wx-hl;
            float min_wy = center_wy-hl;
            float max_wx = center_wx+hl;
            float max_wy = center_wy+hl;

            for(float x = min_wx;x<=max_wx;x+=0.05){
                for(float y=min_wy;y<=max_wy;y+=0.05){
                    int rx,ry;
                    if( !costmap_->worldToRawMap (x,y,rx,ry) )
                        continue;
                    if(costmap_->getRawCost(rx,ry) != MAPCOSTS::OBSTACLE)
                        continue;
                    int time=0;
                    Coordinate pt;
                    pt.x = x; pt.y = y;
                    if(isPointInRect(pt,lt,rt,rb,lb,time)){
                        return true;
                    }

                }
            }

            int min_x,min_y,max_x,max_y  ;
            if( !costmap_->worldToRawMap (center_wx-hl,center_wy-hl,min_x,min_y) )
                return true;
            if( !costmap_->worldToRawMap (center_wx+hl,center_wy+hl,max_x,max_y) )
                return true;
        }

        return collide;
    }

   bool MoyingMap::checkCollision(float length,float width,float center_x,float center_y,float theta,VehiclePtr vehicle){
       Vehicle* veh = nullptr;
       veh = vehicle == nullptr ? nullptr : vehicle.get();
       return checkCollision(length,width,center_x,center_y,theta,veh);
   }

    bool MoyingMap::checkCollision(float length,float width,float center_x,float center_y,float theta,Vehicle* vehicle){

        Coordinate lt,rt,rb,lb;
        double wx,wy;
        wx = center_x;
        wy = center_y;
        //机器人水平朝向的矩形坐标
        lb.x = wx - length/2;  lb.y = wy - width/2;
        rt.x = wx + length/2;  rt.y = wy + width/2;
        lt.x = lb.x;    lt.y = rt.y;
        rb.x = rt.x;    rb.y = lb.y;

        //机器人旋转后的矩形坐标
        float x,y;
        x = lt.x; y = lt.y;
        lt.x = (x-wx)*cos(theta) - (y-wy)*sin(theta) + wx;
        lt.y = (x-wx)*sin(theta) + (y-wy)*cos(theta) + wy;

        x = rt.x; y = rt.y;
        rt.x = (x-wx)*cos(theta) - (y-wy)*sin(theta) + wx;
        rt.y = (x-wx)*sin(theta) + (y-wy)*cos(theta) + wy;

        x = rb.x; y = rb.y;
        rb.x = (x-wx)*cos(theta) - (y-wy)*sin(theta) + wx;
        rb.y = (x-wx)*sin(theta) + (y-wy)*cos(theta) + wy;

        x = lb.x; y = lb.y;
        lb.x = (x-wx)*cos(theta) - (y-wy)*sin(theta) + wx;
        lb.y = (x-wx)*sin(theta) + (y-wy)*cos(theta) + wy;
        
        int r = (length/2+0.1)/costmap_->getRawResolution();
        int rx,ry;
        costmap_->worldToRawMap(center_x,center_y,rx,ry);

        int min_x = rx-r;
        int max_x = rx+r;
        int min_y = ry-r;
        int max_y = ry+r;

        int raw_size_x = costmap_->getRawSizeInCellsX();
        int raw_size_y = costmap_->getRawSizeInCellsY();
        min_x = min_x < 0 ? 0 : min_x;
        min_y = min_y < 0 ? 0 : min_y;
        max_x = max_x >= raw_size_x ? raw_size_x-1 : max_x;
        max_y = max_y >= raw_size_y ? raw_size_y-1 : max_y;
        auto raw_costmap =  costmap_->get_raw_cost_map();

        moying_proto::LocalObstacle* local_obstacle_ptr_ = nullptr;

        for(int x=min_x;x<max_x;x++){
            for(int y=min_y;y<max_y;y++){

                if((*raw_costmap)[x][y] != MAPCOSTS::OBSTACLE && vehicle == nullptr)
                    continue;
                    
                //检测点是否在矩形中
                Coordinate pt;
                costmap_->rawMapToWorld(x,y,pt.x,pt.y);

                int time;
                if( !isPointInRect(pt,lt,rt,rb,lb,time) )
                    continue;

                if((*raw_costmap)[x][y] == MAPCOSTS::OBSTACLE)
                    return true;
                
            }
        }

        return false;

    }

    bool MoyingMap::checkSquareEdgeIsObstacle(float center_wx,float center_wy,float edge_len){

        int rmx,rmy;
        if( !costmap_->worldToRawMap(center_wx,center_wy,rmx,rmy) )
            return true;
        float raw_res = costmap_->getRawResolution();
        if(raw_res == 0)
            return true;
        int half_raw_len = ceil(edge_len / raw_res / 2);
        int rx,ry;

        //下边
        rx = rmx-half_raw_len;
        ry = rmy - half_raw_len;
        for(int i=0;i<=half_raw_len*2;i++){
            if(costmap_->getRawCost(rx+i,ry) == MAPCOSTS::OBSTACLE)
                return true;
        }
        
        //上边
        rx = rmx-half_raw_len;
        ry = rmy + half_raw_len;
        for(int i=0;i<=half_raw_len*2;i++){
            if(costmap_->getRawCost(rx+i,ry) == MAPCOSTS::OBSTACLE)
                return true;
        }

        //左边
        rx = rmx-half_raw_len;
        ry = rmy - half_raw_len;
        for(int i=0;i<half_raw_len*2;i++){
            if(costmap_->getRawCost(rx,ry+i) == MAPCOSTS::OBSTACLE)
                return true;
        }

        //右边
        rx = rmx+half_raw_len;
        ry = rmy - half_raw_len;
        for(int i=0;i<half_raw_len*2;i++){
            if(costmap_->getRawCost(rx,ry+i) == MAPCOSTS::OBSTACLE)
                return true;
        }
        return false;
    }

    bool MoyingMap::getCellNavCoor(int mx, int my, double& wx, double& wy,VehiclePtr vehicle){

        if(vehicle == nullptr)
            return false;
        auto cci = corrected_cells(vehicle);
        if(cci == nullptr){
            printf("cci is nullptr\n");
            return false;
        }

        int x = mx;
        int y = my;
        if( (*cci)[x][y].cell_x != -1 ){   //使用更改过的导航坐标
            int raw_map_x = (*cci)[x][y].raw_map_x;
            int raw_map_y = (*cci)[x][y].raw_map_y;
            costmap_->rawMapToWorld(raw_map_x, raw_map_y, wx, wy);

        }else{
            costmap_->mapToWorld(x,y,wx,wy);
        }
        return true;
    }

    bool MoyingMap::getCellNavCoor(int mx, int my, double& wx, double& wy,unsigned int vehicle_id){

        VehiclePtr vehicle = getVehicle(vehicle_id);
        return getCellNavCoor(mx,my,wx,wy,vehicle);

    }

    void MoyingMap::ergodicRobots(ErgodicRobotCallback cb){

        if(cb == nullptr)
            return;
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        for(auto item : vehicles_){
            if(!cb(item.second))
                break;
        }
    }
    
} // namespace navigation
} // namespace moying

